﻿#ifndef IFKINEMATICSHANDLE_H
#define IFKINEMATICSHANDLE_H

#include "global.h"     // Joint

#include <Eigen/Dense>


class IFKinematicsHandle : public QObject
{
    Q_OBJECT

    public:

        static IFKinematicsHandle* getInstance();

        void setRobotParameter(double d1, double a1, double a2, double a3, double d4);

        void getRobotParameter(double& d1, double& a1, double& a2, double& a3, double& d4);

        void getRobotJointLimit(QVector<float>& Joint_MIN, QVector<float>& Joint_MAX);

        void getRobotTheta(QVector<float>& theta);

        inline Eigen::Matrix4d getTCFMatrix() const
        {
            return tcf;
        }

        std::vector<float> getOffsetFromZeroPoint(const JointType& joint);

        Eigen::Vector3d getPositionInWorld(const Joint & joint);

        /** 机器人正解(包含了机器人转世界坐标系和tcf矩阵) */
        Eigen::Matrix4d fKine(const Joint & joint) const;

        /** 机器人逆解(包含了机器人转世界坐标系和tcf矩阵) */
        Joint iKine(const Eigen::Matrix4d & pose);

    public slots:

        void recvParameters(QVector<float>);

    private:

        IFKinematicsHandle();

        IFKinematicsHandle(const IFKinematicsHandle&);

        IFKinematicsHandle& operator=(const IFKinematicsHandle&);

        /** 单纯的机器人逆解 */
        Joint iKineRobot(const Eigen::Matrix4d & pose, Joint lastJoints);

        /** 单纯的机器人正解 */
        Eigen::Matrix4d fKineRobot(const Joint & joint) const;



    private:

        static IFKinematicsHandle* instance;

        float d1;   /// 机器人DH参数
        float a1;
        float a2;
        float a3;
        float d4;

        Eigen::Matrix4d tcf;


        QVector<float> Joint_MIN;   /// 机器人各关节运动范围
        QVector<float> Joint_MAX;

        QVector<float> theta;       /// 机器人关节的初始角度θ

};

#endif // IFKINEMATICSHANDLE_H
